#ifndef CAROS_KUKA_LWR_H
#define CAROS_KUKA_LWR_H

#include <geometry_msgs/WrenchStamped.h>
#include <ros/ros.h>
#include <unistd.h>
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <cstdlib>
#include <deque>
#include <fstream>
#include <iostream>
#include <rw/math.hpp>
#include <string>
#include <thread>  // NOLINT(build/c++11)
#include <vector>

#include <caros/kuka_lwr_msgs.h>

#include <caros/common_robwork.h>
#include <caros_control_msgs/RobotState.h>

class logger
{
 private:
  std::ofstream file;
  ros::Time action_time;
  ros::Time total_freq;

  void reset_tf()
  {
    total_freq = ros::Time::now();
  }

 public:
  explicit logger(std::string filename) : file(filename)
  {
    reset_at();
    reset_tf();
    file << "action time, total frequency" << std::endl;
  }

  void reset_at()
  {
    action_time = ros::Time::now();
  }
  void print()
  {
    file << (ros::Time::now() - action_time).toSec() << ", " << 1.0 / (ros::Time::now() - total_freq).toSec()
         << std::endl;
    reset_tf();
  }
  ~logger()
  {
    file.close();
  }
};

class bool_timer
{
 private:
  ros::Time timeout;
  ros::Duration length;
  bool state;
  bool check()
  {
    if (timeout < ros::Time::now())
    {
      state = false;
    }
    return state;
  }

 public:
  explicit bool_timer(bool start_state = false, double delay = 3.0) : timeout(), length(delay), state(start_state)
  {
  }
  operator bool()
  {
    return this->check();
  }
  bool_timer &operator=(const bool &rhs)
  {
    this->state = rhs;
    if (rhs)
    {
      this->update_time();
    }
    return *this;
  }
  void update_time()
  {
    timeout = ros::Time::now() + length;
  }
};

class kuka_lwr
{
  /**
   * @brief kuka_lwr
   * Ros publisher class for the current state
   * of a kuka light weight robot.
   */
 private:
  geometry_msgs::WrenchStamped _current_cart_force;
  rw::math::Q _current_force_q;
  rw::math::Q _current_q;
  rw::math::Transform3D<double> _current_pos;
  bool _is_moving;
  ros::NodeHandle _n;
  ros::Publisher _cart_force_pub;
  ros::Publisher _device_state;
  ros::Publisher _cart_pos_pub;
  ros::Time _delay;
  //     logger _time_log;
 public:
  bool_timer new_message_;
  bool success_;

 public:
  explicit kuka_lwr(std::string topic);

  void publish();
  caros_control_msgs::RobotState to_caros();
  /**
   * @brief For all setters
   * Each setter is constructed to take a vector of strings and convert the next values to the data structure.
   * The format of the string is <label> : a : b : c : d : ...
   * This means the first value is the label name and the consecutive values are formatted.
   */

  /**
   * @brief set_cart_force
   * @inputs variables containing a string with the expected formatting
   * cart_ft : fx : fy : fz : tx : ty : tz
   * @inputs start_index
   * The index of the label
   */
  void set_cart_force(std::vector<std::string> &variables, size_t start_index);
  /**
   * @brief set_force_q
   * @inputs variables containing a string with the expected formatting
   * pos_q : t1 : t2 : t3 : t4 : t5 : t6 : t7
   * values are in [Nm]
   * @inputs start_index
   * The index of the label
   */
  void set_force_q(std::vector<std::string> &variables, size_t start_index);
  /**
   * @brief set_q
   * @inputs variables containing a string with the expected formatting
   * pos_q : q1 : q2 : q3 : q4 : q5 : q6 : q7
   * values are in [rad]
   * @inputs start_index
   * The index of the label
   */
  void set_q(std::vector<std::string> &variables, size_t start_index);
  /**
   * @brief set_pos
   * @inputs variables containing a string with the expected formatting
   * pos : x : y : z : r : p : y
   * Position is in [mm], rotation is in [rad]
   * @inputs start_index
   * The index of the label
   */
  void set_pos(std::vector<std::string> &variables, size_t start_index);

  void set_moving(bool is_moving);

  friend std::ostream &operator<<(std::ostream &os, const kuka_lwr &measurement)
  {
    os << measurement._current_cart_force << '\n'
       << measurement._current_force_q << '\n'
       << measurement._current_pos << '\n'
       << measurement._current_q << '\n'
       << (measurement._is_moving ? "moving" : "stopped");
    return os;
  }
};

#endif  // CAROS_KUKA_LWR_H
